#include"dwe.h"

#define GRAVITATIONAL_ACC 9.794 // The gravity of Shanghai

using namespace Eigen;
using namespace std;

// nlink_parser::LinktrackNodeframe3 UWB_cb_;
nlink_parser::LinktrackNodeframe2 UWB_cb_;
geometry_msgs::PoseStamped VICON_posUAV_cb_;
geometry_msgs::TwistStamped VICON_velUAV_cb_;

int dwe::lopt_;

MatrixXd dwe::camA_;
MatrixXd dwe::camB_;
MatrixXd dwe::camC_;
MatrixXd dwe::camD_;

MatrixXd dwe::camE_;
MatrixXd dwe::camF_;
MatrixXd dwe::camG_;
MatrixXd dwe::camH_;

MatrixXd dwe::VICON_PosUAV_;
MatrixXd dwe::VICON_PosUWB_;



/**
 * @brief 构造函数 Construct a new dwe::dwe object
 */
dwe::dwe(ros::NodeHandle& nh) {
    ROS_INFO("**********START ROS NODE**********");

    const std::string& node_name = ros::this_node::getName();
    nh.getParam(node_name + "/" + "delta", delta_);
    nh.getParam(node_name + "/" + "lopt", lopt_);

    nh.getParam(node_name + "/" + "paramP", paramP_);
    nh.getParam(node_name + "/" + "paramQ", paramQ_);
    nh.getParam(node_name + "/" + "paramR", paramR_);
    nh.getParam(node_name + "/" + "paramU", paramU_);
    nh.getParam(node_name + "/" + "paramUz", paramUz_);

    nh.getParam(node_name + "/" + "bias_imu", bias_imu_);
    nh.getParam(node_name + "/" + "order", order_);
    nh.getParam(node_name + "/" + "uwbnum", uwbnum_);
    nh.getParam(node_name + "/" + "useduwbnum", useduwbnum_);
    nh.getParam(node_name + "/" + "vicon_flag", vicon_flag_);
    nh.getParam(node_name + "/" + "dwe_flag", dwe_flag_);

    vector<double> kappa_vec(3);
    nh.getParam(node_name + "/" + "kappa", kappa_vec);
    for (int i = 0; i < 3; i++) {
        kappa_(i) = kappa_vec[i];
    }

    vector<double> bias_pos_vec(3);
    nh.getParam(node_name + "/" + "bias_pos", bias_pos_vec);
    for (int i = 0; i < 3; i++) {
        bias_pos_(i) = bias_pos_vec[i];
    }

    vector<double> pos_uwb_init_vec(12);
    nh.getParam(node_name + "/" + "pos_uwb_init", pos_uwb_init_vec);
    for (int i = 0; i < 3; i++) {
        pos_uwb_init_(i,0) = pos_uwb_init_vec[4 * i];
        pos_uwb_init_(i,1) = pos_uwb_init_vec[4 * i+1];
        pos_uwb_init_(i,2) = pos_uwb_init_vec[4 * i+2];
        pos_uwb_init_(i,3) = pos_uwb_init_vec[4 * i+3];
    }
    
    vector<double> pos_uav_init_vec(3);
    nh.getParam(node_name + "/" + "pos_uav_init", pos_uav_init_vec);
    for (int i = 0; i < 3; i++) {
        pos_uav_init_(i) = pos_uav_init_vec[i];
    }

    vector<double> pos_uwb01_init_vec(3);
    nh.getParam(node_name + "/" + "pos_uwb01_init", pos_uwb01_init_vec);
    for (int i = 0; i < 3; i++) {
        pos_uwb01_init_(i) = pos_uwb01_init_vec[i];
    }
    
    // Initialize member param
    // paramUWB_.assign(uwbnum, paramR);
    // vector<double> paramUWB_(uwbnum,paramR);

    // 定义 paramUWB_ 矩阵
    paramUWB_.setOnes(uwbnum_,k_*lopt_+1);
    paramUWB_.setConstant(paramR_);
    paramUWB_est_.setOnes(3,k_*lopt_+1);
    paramUWB_est_.setConstant(paramU_);
    pos_uav_init_ = pos_uav_init_ - pos_uwb01_init_;
    for (int j=1;j<=4;j++){
        pos_uwb_init_.col(j-1) = pos_uwb_init_.col(j-1) - pos_uwb01_init_;
    }

    count_j_ = k_ * lopt_ + delta_;  // 12+3
    LOOP_num_ = 1;
    startNum_ = 10*lopt_; // start befor takeoff
    
    // Initialize matrix
    // MHE_flow_.setOnes(3,k_*lopt_+1);
    MHE_imu_.setOnes(3,k_*lopt_+1);
    MHE_imu_att_.setOnes(4,k_*lopt_+1);
    MHE_uwb_.setZero(uwbnum_,k_*lopt_+1);
    MHE_uwb_pos_.setZero(3,k_*lopt_+1);
    MHE_uwb_posorigin_.setZero(3,k_*lopt_+1);
    MHE_uwb_sensor_.setZero(uwbnum_,k_*lopt_+1);
    deltaTime_.setZero(1,k_*lopt_+1);
    // MHE_height_.setZero(1,k_*lopt_+1);
    xt_.setOnes(6,1);
    xt_real_.setOnes(6,1);
    xt_camera.setOnes(6,1);
    // xt_pre_.setOnes(6,k_*lopt_+1);
    x1_.setOnes(6,k_*lopt_+1);

    VICON_inital_.setZero(3,1);
    xt_inital_.setZero(3,1);
    R_qua2mat.setOnes(3,3);

    // to do
    VICON_PosUAV_.setZero(3,k_*lopt_+1);
    VICON_PosUWB_.setZero(3*uwbnum_,k_*lopt_+1);
    // VICON_PosUWB02_.setOnes(3,k_*lopt_+1);
    // VICON_PosUWB03_.setOnes(3,k_*lopt_+1);
    // VICON_PosUWB04_.setOnes(3,k_*lopt_+1);

    camA_.setZero(3,k_*lopt_+1);
    camB_.setZero(3,k_*lopt_+1);
    camC_.setZero(3,k_*lopt_+1);
    camD_.setZero(3,k_*lopt_+1);
    camE_.setZero(3,k_*lopt_+1);
    camF_.setZero(3,k_*lopt_+1);
    camG_.setZero(3,k_*lopt_+1);
    camH_.setZero(3,k_*lopt_+1);

    cameraA_vector_last.setZero(3,k_*lopt_+1);

    VICON_VelUAV_.setOnes(3,k_*lopt_+1);
    VICON_PosUWB_ini.setOnes(3,uwbnum_);
    // VICON_vel02_.setOnes(3,k_*lopt_+1);
    VICON_vel_.setOnes(3,k_*lopt_+1);
    VICON_ImuUAV_.setOnes(3,k_*lopt_+1);
    VICON_uwb_.setZero(uwbnum_,k_*lopt_+1); 
    VICON_uwb_ini.setZero(3,uwbnum_); 
    VICON_PosUAV2UWB01_.setOnes(3,1);

    // VICON_UAV_cb_.setOnes(6,1);
    // VICON_UWB_cb_.setOnes(3,uwbnum_);

    // Initialize matrix in solveonce
    Qp.setOnes(1,3);
    Qv.setOnes(1,3);

    Pp.setOnes(1,3);
    // Pp << 1, 1, 10;
    Pv.setOnes(1,3);

    tP << Pp,Pv;
    P = tP.asDiagonal()*paramP_;

    tQ << Qp,Qv;
    Q = tQ.asDiagonal()*paramQ_;

    num_state_= k_*lopt_; //20

    Ex.setZero(47*num_state_, 6*(num_state_+1));       //320x126
    Eu.setZero(47*num_state_, 44*num_state_);          //320x260
    W.setZero(47*num_state_, 47*num_state_);           //320x320
    ur.setZero(44*num_state_,1);                       //260x1
    xxe.setZero(6,lopt_);                              //6x20

    t_matrix.setZero(6*(num_state_+1),6*(order_+1));   //126x30
    for (int j=1;j<=6;j++){
        t_matrix(j-1,(j-1)*(order_+1)) = 1;
    }

}

/**
 * @brief 析构函数
 */
dwe::~dwe(){
}

// void dwe::uwb3_cb(nlink_parser::LinktrackNodeframe3 msg){
//     // ROS_WARN("----------uwb----------");
//     UWB_cb_ = msg;
//     // distinguish from the higher anchor
//     // paramUWB_(3,lopt_) = 2*paramUWB_(3,lopt_);
//     // paramUWB_ = uwb_parm;
//     // ROS_INFO("paramUWB_[0] %f", paramUWB_[0]);
//     // ROS_INFO("paramUWB_[0] %f", paramUWB_[1]);
//     // ROS_INFO("paramUWB_[0] %f", paramUWB_[2]);
//     // ROS_INFO("paramUWB_[0] %f", paramUWB_[3]);

// }

void dwe::uwb2_cb(const nlink_parser::LinktrackNodeframe2::ConstPtr &msg){
    // ROS_WARN("----------uwb----------");
    UWB_cb_ = *msg;
    // distinguish from the higher anchor
    // paramUWB_(3,lopt_) = 2*paramUWB_(3,lopt_);
    // paramUWB_ = uwb_parm;
    // ROS_INFO("paramUWB_[0] %d", UWB_cb_.id);
    // ROS_INFO("paramUWB_[0] %f", paramUWB_[1]);
    // ROS_INFO("paramUWB_[0] %f", paramUWB_[2]);
    // ROS_INFO("paramUWB_[0] %f", paramUWB_[3]);

}


// main cb
void dwe::imu_new_cb(const sensor_msgs::Imu::ConstPtr &msg){
    // ROS_WARN("----------imu----------");
    flag_init_imu_ = true;  // check if the hz = 25 ?

    sensor_msgs::Imu imu_msg;
    imu_msg = *msg;

    MHE_imu_.leftCols(lopt_)=MHE_imu_.rightCols(lopt_);
    MHE_imu_(0,lopt_)=imu_msg.linear_acceleration.x * GRAVITATIONAL_ACC;
    MHE_imu_(1,lopt_)=imu_msg.linear_acceleration.y * GRAVITATIONAL_ACC;
    MHE_imu_(2,lopt_)=imu_msg.linear_acceleration.z * GRAVITATIONAL_ACC;

    MHE_imu_att_(0, lopt_) = imu_msg.orientation.w;
    MHE_imu_att_(1, lopt_) = imu_msg.orientation.x;
    MHE_imu_att_(2, lopt_) = imu_msg.orientation.y;
    MHE_imu_att_(3, lopt_) = imu_msg.orientation.z;

    MHE_imu_.col(lopt_) = qua2mat()* MHE_imu_.col(lopt_);
    MHE_imu_(2,lopt_)=MHE_imu_(2,lopt_) + bias_imu_;

    // MHE_imu_(2,lopt_)=MHE_imu_(2,lopt_) - 9.6;
    // MHE_imu_(2,lopt_) = - MHE_imu_(2,lopt_);
    // ROS_WARN("----------5----------");

    for (int i = 1; i <= 3; i++) {
        // tiao bian
        double diff_imu = abs(MHE_imu_(i-1, lopt_) - MHE_imu_(i-1, lopt_ - 1));
        double diff0_imu = abs(MHE_imu_(i-1, lopt_) - MHE_imu_(i-1, 0));

        if (hasStartEst_) { // If the drone has taken off and diff0 is not zero
            if (diff_imu >= 10.0 && diff0_imu != 0) { // If the difference is big
                MHE_imu_(i-1, lopt_) = MHE_imu_(i-1, lopt_ - 1); // Set current value to the previous value
            }
            // else if (diff_imu >= 1.0) { // If the difference is small or long time lost
            //     // MHE_imu_.row(i-1) = meanFilter(MHE_imu_.row(i-1)); // Apply mean filter
            //     MHE_imu_(i-1, lopt_) = (MHE_imu_(i-1, lopt_) + MHE_imu_(i-1, lopt_-1)) /2; // Apply mean filter
            //     // Eigen::MatrixXd MHE_imu_meanFilter = meanFilter(MHE_imu_.row(i-1));
            //     // MHE_imu_(i-1, lopt_)= MHE_imu_meanFilter(0,lopt_);
            // }
        }
    }

    // Calculate the dt_
    currentTime_= imu_msg.header.stamp.sec + imu_msg.header.stamp.nsec/1000000000.0;

    if (previousTime_==0) {
        // First message received, initialize previousTime
        previousTime_ = currentTime_;
    }
    deltaTime_.leftCols(lopt_)=deltaTime_.rightCols(lopt_);
    deltaTime_ (0,lopt_)= currentTime_ - previousTime_;
    previousTime_ = currentTime_;

    // get dt_ 
    // LOOP_num
    if(LOOP_num_ <= lopt_+1){
        dt_ = deltaTime_.sum()/ LOOP_num_;
    }
    else{
        dt_ = deltaTime_.sum()/ deltaTime_.size();
    }
    // ROS_WARN("----------6----------");
    // replace the initial data until takeoff (z<=0.25m)


    //****************
    // start estimate condition
    
    // self takeoff
    if ( !hasStartEst_ && LOOP_num_ > count_j_ +1) {
        if(LOOP_num_ == startNum_){
            hasStartEst_ = true;
        }
    }

    // takeoff vicon
    // if ( !hasStartEst_ && LOOP_num_ > count_j_ +1) {
    //     if(abs(VICON_PosUAV_(0, lopt_) - VICON_inital_(0,0)) >= 0.2 || abs(VICON_PosUAV_(1, lopt_) - VICON_inital_(1,0)) >= 0.2 || abs(VICON_PosUAV_(2, lopt_) - VICON_inital_(2,0)) >= 0.2){
    //         hasStartEst_ = true;
    //     }
    // }
    // if(LOOP_num_ == count_j_ +1){
    //     double sum_imux_ = MHE_imu_.row(0).sum();
    //     double sum_imuy_ = MHE_imu_.row(1).sum();
    //     double sum_imuz_ = MHE_imu_.row(2).sum();
    //     VICON_inital_(0,0) = sum_imux_ / VICON_PosUAV_.cols();
    //     VICON_inital_(1,0) = sum_imuy_ / VICON_PosUAV_.cols();
    //     VICON_inital_(2,0) = sum_imuz_ / VICON_PosUAV_.cols();
    // }

    // takeoff imu
    // if ( !hasStartEst_ && LOOP_num_ > count_j_ +1) {
    //     if(abs(MHE_imu_(0, lopt_)) >= 0.05 || abs(MHE_imu_(1, lopt_)) >= 0.05 || abs(MHE_imu_(2, lopt_)) >= 0.05){
    //         hasStartEst_ = true;
    //     }
    // }

    if( !hasStartEst_ ){
        // InitialWithParam();
        Initialdataxt();
        // ROS_WARN("----------7----------");
        // data_writter();

        ROS_INFO("Initialized data: x:%f y:%f z:%f", xt_real_(0, LOOP_num_ - 1), xt_real_(1, LOOP_num_ - 1), xt_real_(2, LOOP_num_ - 1));
        ROS_INFO("dt: %.4lf seconds", dt_);
        ROS_INFO("---------- Initialize ----------");
    }
    else{
        flag_init_ok_ = true; 

        // cout << VICON_PosUWB_ini(0,0) << "xx" << VICON_PosUWB_ini(1,0) << "xx" << VICON_PosUWB_ini(2,3)<< endl;

        ROS_WARN("----------- Start Estimate -----------");
    }

    // get data for every spin


    // takeoff condition
    // if( !hasTakenOff_ ){
    //     Initialdataxt();
    //     // ROS_WARN("----------7----------");
    //     // data_writter();

    //     ROS_INFO("Initialized data: x:%f y:%f z:%f", xt_real_(0, LOOP_num_ - 1), xt_real_(1, LOOP_num_ - 1), xt_real_(2, LOOP_num_ - 1));
    //     ROS_INFO("dt: %.4lf seconds", dt_);
    //     ROS_INFO("---------- Initialize ----------");
    // }
    // else{
    //     flag_init_ok_ = true; // 无需高度检查，已经起飞
    //     ROS_WARN("----------- Take off -----------");
    // }



    // MHE_imu_(0,lopt_)=MHE_imu_(0,lopt_) - 0.5;
    // MHE_imu_(0,lopt_)=MHE_imu_(0,lopt_) + 1;

    // bias 
    // MHE_imu_(0,lopt_)=MHE_imu_(0,lopt_) - 0.3;

    // sgolayfilt
    // MHE_imu_ = sgolayfilt(MHE_imu_);  
    // ROS_INFO("MHE_imu----CHECK------=");
}

/**
 * @brief callback from csj01 -- dynamic uav
 */
void dwe::pos_uav_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){  // dynamic uav

    VICON_posUAV_cb_ = *msg;
}

void dwe::vel_uav_cb(const geometry_msgs::TwistStamped::ConstPtr &msg){       // dynamic uav

    VICON_velUAV_cb_ = *msg;
}

/**
 * @brief callback from UWB01-08
 */
void dwe::pos_uwba0_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){  // static uav

    geometry_msgs::PoseStamped pos_uwba0_msg;
    pos_uwba0_msg = *msg;

    VICON_PosUWB_.block(0,0,3,lopt_)=VICON_PosUWB_.block(0,1,3,lopt_);
    VICON_PosUWB_(0,lopt_)=pos_uwba0_msg.pose.position.x;
    VICON_PosUWB_(1,lopt_)=pos_uwba0_msg.pose.position.y;
    VICON_PosUWB_(2,lopt_)=pos_uwba0_msg.pose.position.z - 1.10;
    // VICON_PosUWB_(2,lopt_)=pos_uwba0_msg.pose.position.z;
}

void dwe::pos_uwba1_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){  // static uav

    geometry_msgs::PoseStamped pos_uwba1_msg;
    pos_uwba1_msg = *msg;

    VICON_PosUWB_.block(3,0,3,lopt_)=VICON_PosUWB_.block(3,1,3,lopt_);
    VICON_PosUWB_(3,lopt_)=pos_uwba1_msg.pose.position.x;
    VICON_PosUWB_(4,lopt_)=pos_uwba1_msg.pose.position.y;
    VICON_PosUWB_(5,lopt_)=pos_uwba1_msg.pose.position.z - 1.10;
    // VICON_PosUWB_(2,lopt_)=pos_uwba1_msg.pose.position.z;
}

void dwe::pos_uwba2_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){

    geometry_msgs::PoseStamped pos_uwba2_msg;
    pos_uwba2_msg = *msg;

    VICON_PosUWB_.block(6,0,3,lopt_)=VICON_PosUWB_.block(6,1,3,lopt_);
    VICON_PosUWB_(6,lopt_)=pos_uwba2_msg.pose.position.x;
    VICON_PosUWB_(7,lopt_)=pos_uwba2_msg.pose.position.y;
    VICON_PosUWB_(8,lopt_)=pos_uwba2_msg.pose.position.z - 1.10;
    // VICON_PosUWB_(8,lopt_)=pos_uwba2_msg.pose.position.z;

}

void dwe::pos_uwba3_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){

    geometry_msgs::PoseStamped pos_uwba3_msg;
    pos_uwba3_msg = *msg;

    VICON_PosUWB_.block(9,0,3,lopt_)=VICON_PosUWB_.block(9,1,3,lopt_);
    VICON_PosUWB_(9,lopt_)=pos_uwba3_msg.pose.position.x;
    VICON_PosUWB_(10,lopt_)=pos_uwba3_msg.pose.position.y;
    VICON_PosUWB_(11,lopt_)=pos_uwba3_msg.pose.position.z - 1.10;
    // VICON_PosUWB_(11,lopt_)=pos_uwba3_msg.pose.position.z;
}

void dwe::pos_uwba4_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){

    geometry_msgs::PoseStamped pos_uwba4_msg;
    pos_uwba4_msg = *msg;

    VICON_PosUWB_.block(12,0,3,lopt_)=VICON_PosUWB_.block(12,1,3,lopt_);
    VICON_PosUWB_(12,lopt_)=pos_uwba4_msg.pose.position.x;
    VICON_PosUWB_(13,lopt_)=pos_uwba4_msg.pose.position.y;
    VICON_PosUWB_(14,lopt_)=pos_uwba4_msg.pose.position.z;
}

void dwe::pos_uwba5_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){

    geometry_msgs::PoseStamped pos_uwba5_msg;
    pos_uwba5_msg = *msg;

    VICON_PosUWB_.block(15,0,3,lopt_)=VICON_PosUWB_.block(15,1,3,lopt_);
    VICON_PosUWB_(15,lopt_)=pos_uwba5_msg.pose.position.x;
    VICON_PosUWB_(16,lopt_)=pos_uwba5_msg.pose.position.y;
    VICON_PosUWB_(17,lopt_)=pos_uwba5_msg.pose.position.z;
}

void dwe::pos_uwba6_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){

    geometry_msgs::PoseStamped pos_uwba6_msg;
    pos_uwba6_msg = *msg;

    VICON_PosUWB_.block(18,0,3,lopt_)=VICON_PosUWB_.block(18,1,3,lopt_);
    VICON_PosUWB_(18,lopt_)=pos_uwba6_msg.pose.position.x;
    VICON_PosUWB_(19,lopt_)=pos_uwba6_msg.pose.position.y;
    VICON_PosUWB_(20,lopt_)=pos_uwba6_msg.pose.position.z;
}


void dwe::pos_uwba7_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){

    geometry_msgs::PoseStamped pos_uwba7_msg;
    pos_uwba7_msg = *msg;

    VICON_PosUWB_.block(21,0,3,lopt_)=VICON_PosUWB_.block(21,1,3,lopt_);
    VICON_PosUWB_(21,lopt_)=pos_uwba7_msg.pose.position.x;
    VICON_PosUWB_(22,lopt_)=pos_uwba7_msg.pose.position.y;
    VICON_PosUWB_(23,lopt_)=pos_uwba7_msg.pose.position.z;
}


void dwe::camA_cb(geometry_msgs::TransformStamped msg){
    camA_.leftCols(lopt_)=camA_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;
    // note down time stamp


    camA_(0,lopt_)=translation.x;
    camA_(1,lopt_)=translation.y;
    camA_(2,lopt_)=translation.z;
    double dispA_x, dispA_y, dispA_z;
    dispA_x = VICON_PosUAV_(0,lopt_) - VICON_PosUWB_(0,lopt_);
    dispA_y = VICON_PosUAV_(1,lopt_) - VICON_PosUWB_(1,lopt_);
    dispA_z = VICON_PosUAV_(2,lopt_) - VICON_PosUWB_(2,lopt_); 

    double deltaA_x, deltaA_y, deltaA_z;
    deltaA_x = abs( dispA_x - camA_(0,lopt_));
    deltaA_y = abs( dispA_y - camA_(1,lopt_));
    deltaA_z = abs( dispA_z - camA_(2,lopt_));
    ROS_INFO("deltaA_x: %f, deltaA_y: %f, deltaA_z: %f", deltaA_x, deltaA_y, deltaA_z);

}



void dwe::camB_cb(geometry_msgs::TransformStamped msg){
    camB_.leftCols(lopt_)=camB_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;


    camB_(0,lopt_)=translation.x;
    camB_(1,lopt_)=translation.y;
    camB_(2,lopt_)=translation.z;

    double dispB_x, dispB_y, dispB_z;
    dispB_x = VICON_PosUAV_(0,lopt_) - VICON_PosUWB_(0,lopt_);
    dispB_y = VICON_PosUAV_(1,lopt_) - VICON_PosUWB_(1,lopt_);
    dispB_z = VICON_PosUAV_(2,lopt_) - VICON_PosUWB_(2,lopt_); 


    double deltaB_x, deltaB_y, deltaB_z;
    deltaB_x = abs(dispB_x - camB_(0,lopt_));
    deltaB_y = abs(dispB_y - camB_(1,lopt_));
    deltaB_z = abs(dispB_z - camB_(2,lopt_));
    ROS_INFO("deltaB_x: %f, deltaB_y: %f, deltaB_z: %f", deltaB_x, deltaB_y, deltaB_z);
}

void dwe::camC_cb(geometry_msgs::TransformStamped msg){
    camC_.leftCols(lopt_)=camC_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;


    camC_(0,lopt_)=translation.x;
    camC_(1,lopt_)=translation.y;
    camC_(2,lopt_)=translation.z;

}

void dwe::camD_cb(geometry_msgs::TransformStamped msg){
    camD_.leftCols(lopt_)=camD_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;

    camD_(0,lopt_)=translation.x;
    camD_(1,lopt_)=translation.y;
    camD_(2,lopt_)=translation.z;

}

void dwe::camE_cb(geometry_msgs::TransformStamped msg){
    camE_.leftCols(lopt_)=camE_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;

    camE_(0,lopt_)=translation.x;
    camE_(1,lopt_)=translation.y;
    camE_(2,lopt_)=translation.z;

}

void dwe::camF_cb(geometry_msgs::TransformStamped msg){
    camF_.leftCols(lopt_)=camF_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;

    camF_(0,lopt_)=translation.x;
    camF_(1,lopt_)=translation.y;
    camF_(2,lopt_)=translation.z;

}

void dwe::camG_cb(geometry_msgs::TransformStamped msg){
    camG_.leftCols(lopt_)=camG_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;

    camG_(0,lopt_)=translation.x;
    camG_(1,lopt_)=translation.y;
    camG_(2,lopt_)=translation.z;

}

void dwe::camH_cb(geometry_msgs::TransformStamped msg){
    camH_.leftCols(lopt_)=camH_.rightCols(lopt_);
    geometry_msgs::Vector3 translation = msg.transform.translation;

    camH_(0,lopt_)=translation.x;
    camH_(1,lopt_)=translation.y;
    camH_(2,lopt_)=translation.z;

}



/**
 * @brief initial data xt_ with vicon
 */
void dwe::Initialdataxt(){
    // resize xt_
    xt_.conservativeResize(6, LOOP_num_);
    xt_real_.conservativeResize(6, LOOP_num_);
    xt_camera.conservativeResize(3,LOOP_num_);
    // initialize loc with nonzero numbers, e.g., x=y=z=uwb.dis/sqrt(3);
    // xt_(0,LOOP_num_-1) = MHE_uwb_(0,lopt_)/sqrt(3);
    // xt_(1,LOOP_num_-1) = MHE_uwb_(0,lopt_)/sqrt(3); 
    // xt_(2,LOOP_num_-1) = MHE_uwb_(0,lopt_)/sqrt(3); 

    // initialize loc with vicon
    xt_(0,LOOP_num_-1) = VICON_PosUAV_(0,lopt_) - VICON_PosUWB_(0,lopt_);  // use UWB01 as initial point
    xt_(1,LOOP_num_-1) = VICON_PosUAV_(1,lopt_) - VICON_PosUWB_(1,lopt_); 
    xt_(2,LOOP_num_-1) = VICON_PosUAV_(2,lopt_) - VICON_PosUWB_(2,lopt_); 
    xt_(3,LOOP_num_-1) = VICON_vel_(0,lopt_);
    xt_(4,LOOP_num_-1) = VICON_vel_(1,lopt_);
    xt_(5,LOOP_num_-1) = VICON_vel_(2,lopt_);

     // initialize loc with vicon
    xt_real_(0,LOOP_num_-1) = VICON_PosUAV_(0,lopt_);
    xt_real_(1,LOOP_num_-1) = VICON_PosUAV_(1,lopt_);
    xt_real_(2,LOOP_num_-1) = VICON_PosUAV_(2,lopt_);
    xt_real_(3,LOOP_num_-1) = VICON_vel_(0,lopt_);
    xt_real_(4,LOOP_num_-1) = VICON_vel_(1,lopt_);
    xt_real_(5,LOOP_num_-1) = VICON_vel_(2,lopt_);


    xt_camera(0,LOOP_num_-1) = VICON_PosUAV_(0,lopt_);
    xt_camera(1,LOOP_num_-1) = VICON_PosUAV_(1,lopt_);
    xt_camera(2,LOOP_num_-1) = VICON_PosUAV_(2,lopt_);


    for(int i =1 ; i<= uwbnum_; i++){
        // get uwb pos ini from vicon
        VICON_PosUWB_ini.col(i-1) = VICON_PosUWB_.block(3*(i-1),lopt_,3,1) - VICON_PosUWB_.block(0,lopt_,3,1);
        // get ini vicon uwb pos to avoid the loss of vicon
        VICON_uwb_ini.col(i-1) = VICON_PosUWB_.block(3*(i-1),lopt_,3,1);
    }

} 
/**
 * @brief initial data xt_ with pre-get param
 */

// todo
void dwe::InitialWithParam(){
    // resize xt_
    xt_.conservativeResize(6, LOOP_num_);
    xt_real_.conservativeResize(6, LOOP_num_);
    xt_camera.conservativeResize(3,LOOP_num_);
    // initialize loc without vicon
    xt_(0,LOOP_num_-1) = pos_uav_init_(0);  // use UWB01 as initial point
    xt_(1,LOOP_num_-1) = pos_uav_init_(1);
    xt_(2,LOOP_num_-1) = pos_uav_init_(2);
    xt_(3,LOOP_num_-1) = 0;
    xt_(4,LOOP_num_-1) = 0;
    xt_(5,LOOP_num_-1) = 0;

     // initialize loc without vicon
    xt_real_(0,LOOP_num_-1) = pos_uav_init_(0) + pos_uwb01_init_(0);
    xt_real_(1,LOOP_num_-1) = pos_uav_init_(1) + pos_uwb01_init_(1);
    xt_real_(2,LOOP_num_-1) = pos_uav_init_(2) + pos_uwb01_init_(2);
    xt_real_(3,LOOP_num_-1) = 0;
    xt_real_(4,LOOP_num_-1) = 0;
    xt_real_(5,LOOP_num_-1) = 0;

    xt_camera(0,LOOP_num_-1) = pos_uav_init_(0) + pos_uwb01_init_(0);
    xt_camera(1,LOOP_num_-1) = pos_uav_init_(1) + pos_uwb01_init_(1);
    xt_camera(2,LOOP_num_-1) = pos_uav_init_(2) + pos_uwb01_init_(2);
} 

/**
 * @brief rotation matrx
 */
MatrixXd dwe::qua2mat(){

    double _w = MHE_imu_att_(0, lopt_);
    double _x = MHE_imu_att_(1, lopt_);
    double _y = MHE_imu_att_(2, lopt_);
    double _z = MHE_imu_att_(3, lopt_);

    R_qua2mat <<1-2*pow(_y,2)-2*pow(_z,2), 2*_x*_y-2*_z*_w, 2*_x*_z+2*_y*_w,
                2*_x*_y+2*_z*_w, 1-2*pow(_x,2)-2*pow(_z,2), 2*_y*_z-2*_x*_w,
                2*_x*_z-2*_y*_w, 2*_y*_z+2*_x*_w, 1-2*pow(_x,2)-2*pow(_y,2);
    return R_qua2mat;    
}

// Define the Savitzky-Golay filter function
MatrixXd dwe::sgolayfilt(Eigen::MatrixXd data){
    int M = M_sg_size_;
    int N = N_sg_poly_;

    // 数据矩阵行数和列数
    int num_rows = data.rows();
    int num_cols = data.cols();
	
	MatrixXd filtered_data = data;
	if (num_cols <= 2 * M + 1){
		ROS_INFO("the length of the input_data is too short!");
	}
 
	if (2 * M + 1 <= N){
		ROS_INFO("window size is too small!");
	}

    // SG平滑矩阵算子	
    MatrixXd move_box,II_matrix,move_box_move,result,mox_box_coef,move_box_diag;
    
    move_box.setZero(2 * M + 1, N);
	for (int i = -M; i <= M; i++){
		for (int j = 0; j <= N-1; j++){
            move_box(i + M, j)= pow(i, j);
		}
	}
    II_matrix = Eigen::MatrixXd::Identity(N,N);
    move_box_move = move_box.transpose() * move_box;
    move_box_diag = II_matrix * move_box_move.inverse();
    mox_box_coef = (move_box * move_box_diag) * move_box.transpose();
    for(int k = 1; k <= num_rows; k++){
        for (int i = M+1; i <= num_cols-M; i++){
            result = mox_box_coef * data.block(k-1,i-M-1,1,2*M+1).transpose();
            filtered_data(k-1,i-1) = result(M,0);
        }
    }

    return filtered_data;
}


// Define the mean filter function
MatrixXd dwe::meanFilter(Eigen::MatrixXd data){
    int numRows = data.rows();
    int numCols = data.cols();

    Eigen::MatrixXd filteredMatrix(numRows, numCols);

    for (int row = 0; row < numRows; ++row) {
        double rowSum = 0.0;
        for (int col = 0; col < numCols; ++col) {
            rowSum += data(row, col);
        }
        double rowMean = rowSum / numCols;

        for (int col = 0; col < numCols; ++col) {
            filteredMatrix(row, col) = rowMean;
        }
    }

    return filteredMatrix;
}

// Define the mean filter function with a sliding window
MatrixXd dwe::meanFilterWithSlidingWindow(Eigen::MatrixXd data, int windowSize) {
    int numRows = data.rows();
    int numCols = data.cols();
    Eigen::MatrixXd filteredMatrix(numRows, numCols);

    for (int row = 0; row < numRows; ++row) {
        for (int col = 0; col < numCols; ++col) {
            double rowSum = 0.0;

            for (int i = std::min(col, numCols - windowSize); i <= std::min(col + windowSize -1, numCols-1); ++i) {
                rowSum += data(row, i);
            }

            double rowMean = rowSum / windowSize;

            filteredMatrix(row, col) = rowMean;
        }
    }

    return filteredMatrix;
}

// MatrixXd dwe::meanFilterWithSlidingWindow(Eigen::MatrixXd data, int windowSize) {
//     int numRows = data.rows();
//     int numCols = data.cols();
//     Eigen::MatrixXd filteredMatrix(numRows, numCols);

//     for (int row = 0; row < numRows; ++row) {
//         for (int col = 0; col < numCols; ++col) {
//             double rowSum = 0.0;

//             for (int i = std::max(0, col - windowSize + 1); i <= col; ++i) {
//                 rowSum += data(row, i);
//             }

//             double rowMean = rowSum / std::min(col + 1, windowSize);

//             filteredMatrix(row, col) = rowMean;
//         }
//     }

//     return filteredMatrix;
// }


/**
 * @brief pre process : to get data
 */
void dwe::get_data(){

    // uwb3********************************************************************* id start at 1
    // MHE_uwb_.leftCols(lopt_)=MHE_uwb_.rightCols(lopt_);
    // MHE_uwb_sensor_.leftCols(lopt_)=MHE_uwb_sensor_.rightCols(lopt_);
    // int uwb_size = UWB_cb_.nodes.size();
    // // ROS_INFO("uwb_size: %d ",uwb_size); 
    // paramUWB_.leftCols(lopt_)=paramUWB_.rightCols(lopt_);

    // // 初始化存储 uwb_id 的 vector
    // vector<int> uwb_id(uwbnum_, -1); // 初始化为-1，表示未找到对应 id
    // vector<double> uwb_parm(uwbnum_, paramR_); // 初始化为paramR_

    // // 遍历所有接收到的数据
    // for (int uwb_si = 1; uwb_si <= uwb_size; uwb_si++) {
    //     // 将数据存储到对应的位置
    //     int id = UWB_cb_.nodes[uwb_si-1].id-1;
    //     uwb_id[id] = id+1; // 标记找到了对应的 id 
    //     MHE_uwb_(id, lopt_) = UWB_cb_.nodes[uwb_si-1].dis;
    //     MHE_uwb_sensor_(id, lopt_) = UWB_cb_.nodes[uwb_si-1].dis;
    //     uwb_parm[id] = paramR_;

    //     // tiao bian
    //     double diff = abs(MHE_uwb_(id, lopt_) - MHE_uwb_(id, lopt_ - 1));
    //     double diff0 = abs(MHE_uwb_(id, lopt_) - MHE_uwb_(id, 0));

    //     if (hasStartEst_) { // If the drone has taken off and diff0 is not zero
    //         if (diff >= 1.0 && diff0 != 0) { // If the difference is big
    //             MHE_uwb_(id, lopt_) = MHE_uwb_(id, lopt_ - 1); // Set current value to the previous value
    //             // uwb_parm[id] = paramR_;
    //             uwb_parm[id] = 0.1*paramR_;
    //         }
    //         else if (diff >= 0.5) { // If the difference is small or long time lost
    //             MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id)); // Apply mean filter
    //             // Eigen::MatrixXd MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(id));
    //             // MHE_uwb_(id, lopt_)= MHE_uwb_meanFilter(0,lopt_);
    //             // MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));

    //             uwb_parm[id] = 0.5*paramR_;
    //             // uwb_parm[id] = paramR_;
    //         }
    //     }

    //     // meanfilter for all
    //     MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));
    //     // Eigen::MatrixXd MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(id));
    //     // MHE_uwb_(id, lopt_)= MHE_uwb_meanFilter(0,lopt_);

    //     // double diff = abs(MHE_uwb_(id, lopt_) - MHE_uwb_(id, lopt_ - 1));
    //     // if (diff >= 0.7 && diff <= 0.8) {  // small tiao bian
    //     //     // Apply mean filter
    //     //     MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));
    //     // }
    //     // else if (diff > 0.8 && hasTakenOff_) { // long time lost or big tiao bian
    //     //     // Discard outliers and use the previous value
    //     //     MHE_uwb_(id, lopt_) = MHE_uwb_(id, lopt_ - 1);
    //     // }

    // }
    // // 检查未填充的数据并处理
    // for (int i = 1; i <= uwbnum_; i++) {
    //     if (uwb_id[i-1] == -1) {
    //         // 找不到对应的 id，说明未收到该数据，将参数置为0
    //         MHE_uwb_(i-1, lopt_) = MHE_uwb_(i-1, lopt_ - 1); // 使用上一时刻的数据
    //         MHE_uwb_sensor_(i-1, lopt_) = MHE_uwb_sensor_(i-1, lopt_ - 1); // 使用上一时刻的数据
    //         uwb_parm[i-1] = 0*paramR_;
    
    //         // meanfilter for all
    //         // MHE_uwb_.row(i-1) = meanFilter(MHE_uwb_.row(i-1));
    //         // MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(i-1));
    //         // MHE_uwb_(i-1, lopt_)= MHE_uwb_meanFilter(0,lopt_);
    //         // ROS_WARN("MHE_uwb[%d] not found in received data", i);  //4
    //     }
    // }
    // // // moni 4 anhors
    // // uwb_parm[6] = uwb_parm[4];
    // // uwb_parm[7] = uwb_parm[5];
    // // MHE_uwb_.row(6) = MHE_uwb_.row(4);
    // // MHE_uwb_.row(7) = MHE_uwb_.row(5);

    // // MHE_uwb_sensor_.row(6) = MHE_uwb_sensor_.row(4);
    // // MHE_uwb_sensor_.row(7) = MHE_uwb_sensor_.row(5);


    // // update paramUWB_
    // // 获取 paramUWB_ 的最后一列，并将其赋值为 uwb_parm
    // Eigen::VectorXd uwb_param_vec = Eigen::Map<Eigen::VectorXd>(uwb_parm.data(), uwb_parm.size());
    // paramUWB_.rightCols(1) = uwb_param_vec;

    // // if ( !check_for_init_) 
    // // {
    // //     paramUWB_.setConstant(0.01);
    // // }

    //uwb2******************************************************************************
    // get nooploop 3d pos
    MHE_uwb_posorigin_.leftCols(lopt_)=MHE_uwb_posorigin_.rightCols(lopt_);
    MHE_uwb_posorigin_(0,lopt_)=UWB_cb_.pos_3d[0];
    MHE_uwb_posorigin_(1,lopt_)=UWB_cb_.pos_3d[1];
    MHE_uwb_posorigin_(2,lopt_)=UWB_cb_.pos_3d[2];

    paramUWB_est_.leftCols(lopt_)=paramUWB_est_.rightCols(lopt_);

    vector<double> uwb_est_parm(3, paramU_); // 初始化为paramR_

    // tiaodian
    MHE_uwb_pos_ = MHE_uwb_posorigin_;
    for (int k = 0; k <= 2; k++) {

        // tiao bian
        double diffuwb = abs(MHE_uwb_pos_(k, lopt_) - MHE_uwb_pos_(k, lopt_ - 1));
        double diffuwb0 = abs(MHE_uwb_pos_(k, lopt_) - MHE_uwb_pos_(k, 0));
        if (hasStartEst_) { // If the drone has taken off and diff0 is not zero
            if (diffuwb >= 0.75 && diffuwb0 != 0) { // If the difference is big
                if(diffuwb0 != 0){
                    MHE_uwb_pos_(k, lopt_) = MHE_uwb_pos_(k, lopt_ - 1); // Set current value to the previous value
                }
                // MHE_uwb_pos_(k, lopt_) = MHE_uwb_pos_(k, lopt_ - 1); // Set current value to the previous value
                // uwb_parm[id] = paramR_;
                uwb_est_parm[k] = 0.1*paramU_;
            }
            else if (diffuwb >= 0.2) { // If the difference is small or long time lost
                // MHE_uwb_pos_.row(k) = meanFilter(MHE_uwb_pos_.row(k)); // Apply mean filter
                MHE_uwb_pos_(k, lopt_) = (MHE_uwb_pos_(k, lopt_ - 1) +  MHE_uwb_pos_(k, lopt_))/2;

                // Eigen::MatrixXd MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(id));
                // MHE_uwb_(id, lopt_)= MHE_uwb_meanFilter(0,lopt_);
                // MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));
                uwb_est_parm[k] = 0.5*paramU_;
                // uwb_parm[id] = paramR_;
            }
        }
        // meanfilter for all
        MHE_uwb_pos_.row(k) = meanFilter(MHE_uwb_pos_.row(k));
    }
    // ROS_WARN("UWB_cb_.pos_3d[0]:%f",UWB_cb_.pos_3d[0]); 
    uwb_est_parm[2] = paramUz_;  // noop_est z = 0
    
    // update paramUWB_est_
    // 获取 paramUWB_est_ 的最后一列，并将其赋值为 uwb_parm
    Eigen::VectorXd uwb_est_param_vec = Eigen::Map<Eigen::VectorXd>(uwb_est_parm.data(), uwb_est_parm.size());
    paramUWB_est_.rightCols(1) = uwb_est_param_vec;

    // get nooploop dis
    MHE_uwb_.leftCols(lopt_)=MHE_uwb_.rightCols(lopt_);
    MHE_uwb_sensor_.leftCols(lopt_)=MHE_uwb_sensor_.rightCols(lopt_);
    int uwb_size = UWB_cb_.nodes.size();
    // ROS_INFO("uwb_size: %d ",uwb_size); 
    paramUWB_.leftCols(lopt_)=paramUWB_.rightCols(lopt_);

    // 初始化存储 uwb_id 的 vector
    vector<int> uwb_id(uwbnum_, -1); // 初始化为-1，表示未找到对应 id
    vector<double> uwb_parm(uwbnum_, paramR_); // 初始化为paramR_

    // 遍历所有接收到的数据
    for (int uwb_si = 1; uwb_si <= uwb_size; uwb_si++) {
        // 将数据存储到对应的位置
        int id = UWB_cb_.nodes[uwb_si-1].id;
        uwb_id[id] = id; // 标记找到了对应的 id 
        MHE_uwb_(id, lopt_) = UWB_cb_.nodes[uwb_si-1].dis;
        MHE_uwb_sensor_(id, lopt_) = UWB_cb_.nodes[uwb_si-1].dis;
        uwb_parm[id] = paramR_;

        // tiao bian
        double diff = abs(MHE_uwb_(id, lopt_) - MHE_uwb_(id, lopt_ - 1));
        double diff0 = abs(MHE_uwb_(id, lopt_) - MHE_uwb_(id, 0));

        if (hasStartEst_) { // If the drone has taken off and diff0 is not zero
            // if (diff >= 1.0 && diff0 != 0) { // If the difference is big
            //     MHE_uwb_(id, lopt_) = MHE_uwb_(id, lopt_ - 1); // Set current value to the previous value
            //     // uwb_parm[id] = paramR_;
            //     uwb_parm[id] = 0.1*paramR_;
            // }
            // else if (diff >= 0.5) { // If the difference is small or long time lost
            //     // MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id)); // Apply mean filter

            //     MHE_uwb_(id, lopt_) = (MHE_uwb_(id, lopt_ - 1) +  MHE_uwb_(id, lopt_))/2;
                
            //     // Eigen::MatrixXd MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(id));
            //     // MHE_uwb_(id, lopt_)= MHE_uwb_meanFilter(0,lopt_);
            //     // MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));

            //     uwb_parm[id] = 0.5*paramR_;
            //     // uwb_parm[id] = paramR_;
            // }

            if (diff >= 0.5 && diff0 != 0) { // If the difference is big
                MHE_uwb_(id, lopt_) = MHE_uwb_(id, lopt_ - 1); // Set current value to the previous value
                uwb_parm[id] = 0.5*paramR_;
            }
        }

        // meanfilter for all
        MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));
        // Eigen::MatrixXd MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(id));
        // MHE_uwb_(id, lopt_)= MHE_uwb_meanFilter(0,lopt_);

        // double diff = abs(MHE_uwb_(id, lopt_) - MHE_uwb_(id, lopt_ - 1));
        // if (diff >= 0.7 && diff <= 0.8) {  // small tiao bian
        //     // Apply mean filter
        //     MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));
        // }
        // else if (diff > 0.8 && hasTakenOff_) { // long time lost or big tiao bian
        //     // Discard outliers and use the previous value
        //     MHE_uwb_(id, lopt_) = MHE_uwb_(id, lopt_ - 1);
        // }

    }
    // 检查未填充的数据并处理
    for (int i = 1; i <= uwbnum_; i++) {
        if (uwb_id[i-1] == -1) {
            // 找不到对应的 id，说明未收到该数据，将参数置为0
            MHE_uwb_(i-1, lopt_) = MHE_uwb_(i-1, lopt_ - 1); // 使用上一时刻的数据
            MHE_uwb_sensor_(i-1, lopt_) = MHE_uwb_sensor_(i-1, lopt_ - 1); // 使用上一时刻的数据
            uwb_parm[i-1] = 0*paramR_;
    
            // meanfilter for all
            // MHE_uwb_.row(i-1) = meanFilter(MHE_uwb_.row(i-1));
            // MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(i-1));
            // MHE_uwb_(i-1, lopt_)= MHE_uwb_meanFilter(0,lopt_);
            // ROS_WARN("MHE_uwb[%d] not found in received data", i);  //4
        }
    }
    // // moni 4 anhors
    // uwb_parm[6] = uwb_parm[4];
    // uwb_parm[7] = uwb_parm[5];
    // MHE_uwb_.row(6) = MHE_uwb_.row(4);
    // MHE_uwb_.row(7) = MHE_uwb_.row(5);

    // MHE_uwb_sensor_.row(6) = MHE_uwb_sensor_.row(4);
    // MHE_uwb_sensor_.row(7) = MHE_uwb_sensor_.row(5);


    // update paramUWB_
    // 获取 paramUWB_ 的最后一列，并将其赋值为 uwb_parm
    Eigen::VectorXd uwb_param_vec = Eigen::Map<Eigen::VectorXd>(uwb_parm.data(), uwb_parm.size());
    paramUWB_.rightCols(1) = uwb_param_vec;

    // if ( !check_for_init_) 
    // {
    //     paramUWB_.setConstant(0.01);
    // }
    //tagframe0******************************************************************************
    // MHE_uwb_.leftCols(lopt_)=MHE_uwb_.rightCols(lopt_);
    // MHE_uwb_sensor_.leftCols(lopt_)=MHE_uwb_sensor_.rightCols(lopt_);
    // paramUWB_.leftCols(lopt_)=paramUWB_.rightCols(lopt_);

    // // get nooploop 3d pos
    // MHE_uwb_pos_.leftCols(lopt_)=MHE_uwb_pos_.rightCols(lopt_);
    // MHE_uwb_pos_(0,lopt_)=UWB_cb_.pos_3d[0];
    // MHE_uwb_pos_(1,lopt_)=UWB_cb_.pos_3d[1];
    // MHE_uwb_pos_(2,lopt_)=UWB_cb_.pos_3d[2];
    // // ROS_WARN("UWB_cb_.pos_3d[0]:%f",UWB_cb_.pos_3d[0]); 

    // // 初始化存储 uwb_id 的 vector
    // vector<int> uwb_id(uwbnum_, -1); // 初始化为-1，表示未找到对应 id
    // vector<double> uwb_parm(uwbnum_, paramR_); // 初始化为paramR_

    // // 遍历所有接收到的数据 get dis
    // for (int uwb_si = 1; uwb_si <= uwbnum_; uwb_si++) {
    //     // 找不到对应的 id，说明未收到该数据，将参数置为0
    //     // ROS_WARN("UWB_cb_.dis_arr[0]:%f",UWB_cb_.dis_arr[0]); 
    //     // ROS_WARN("UWB_cb_.dis_arr[1]:%f",UWB_cb_.dis_arr[1]); 
    //     // ROS_WARN("UWB_cb_.dis_arr[2]:%f",UWB_cb_.dis_arr[2]); 
    //     if(UWB_cb_.dis_arr[uwb_si-1] == 0){
    //         MHE_uwb_(uwb_si-1, lopt_) = MHE_uwb_(uwb_si-1, lopt_ - 1); // 使用上一时刻的数据
    //         uwb_parm[uwb_si-1] = 0*paramR_;
    //     }
    //     else{
    //         MHE_uwb_(uwb_si-1, lopt_) = UWB_cb_.dis_arr[uwb_si-1];
    //         MHE_uwb_sensor_(uwb_si-1, lopt_) = UWB_cb_.dis_arr[uwb_si-1];
    //         uwb_parm[uwb_si] = paramR_;

    //         // tiao bian
    //         double diff = abs(MHE_uwb_(uwb_si-1, lopt_) - MHE_uwb_(uwb_si-1, lopt_ - 1));
    //         double diff0 = abs(MHE_uwb_(uwb_si-1, lopt_) - MHE_uwb_(uwb_si-1, 0));

    //         if (hasStartEst_) { // If the drone has taken off and diff0 is not zero
    //             if (diff >= 1.0 && diff0 != 0) { // If the difference is big
    //                 MHE_uwb_(uwb_si-1, lopt_) = MHE_uwb_(uwb_si-1, lopt_ - 1); // Set current value to the previous value
    //                 // uwb_parm[id] = paramR_;
    //                 uwb_parm[uwb_si-1] = 0.1*paramR_;
    //             }
    //             else if (diff >= 0.5) { // If the difference is small or long time lost
    //                 MHE_uwb_.row(uwb_si-1) = meanFilter(MHE_uwb_.row(uwb_si-1)); // Apply mean filter
    //                 // Eigen::MatrixXd MHE_uwb_meanFilter = meanFilter(MHE_uwb_.row(id));
    //                 // MHE_uwb_(id, lopt_)= MHE_uwb_meanFilter(0,lopt_);
    //                 // MHE_uwb_.row(id) = meanFilter(MHE_uwb_.row(id));

    //                 uwb_parm[uwb_si-1] = 0.5*paramR_;
    //                 // uwb_parm[id] = paramR_;
    //             }
    //         }

    //     }
    //     MHE_uwb_.row(uwb_si-1) = meanFilter(MHE_uwb_.row(uwb_si-1)); // Apply mean filter

    // }

    // // // moni 4 anhors
    // // uwb_parm[6] = uwb_parm[4];
    // // uwb_parm[7] = uwb_parm[5];
    // // MHE_uwb_.row(6) = MHE_uwb_.row(4);
    // // MHE_uwb_.row(7) = MHE_uwb_.row(5);

    // // MHE_uwb_sensor_.row(6) = MHE_uwb_sensor_.row(4);
    // // MHE_uwb_sensor_.row(7) = MHE_uwb_sensor_.row(5);


    // // update paramUWB_
    // // 获取 paramUWB_ 的最后一列，并将其赋值为 uwb_parm
    // Eigen::VectorXd uwb_param_vec = Eigen::Map<Eigen::VectorXd>(uwb_parm.data(), uwb_parm.size());
    // paramUWB_.rightCols(1) = uwb_param_vec;

    // // if ( !check_for_init_) 
    // // {
    // //     paramUWB_.setConstant(0.01);
    // // }

    // vicon posuav*********************************************************************
    VICON_PosUAV_.leftCols(lopt_)=VICON_PosUAV_.rightCols(lopt_);
    VICON_PosUAV_(0,lopt_)=VICON_posUAV_cb_.pose.position.x;
    VICON_PosUAV_(1,lopt_)=VICON_posUAV_cb_.pose.position.y;
    VICON_PosUAV_(2,lopt_)=VICON_posUAV_cb_.pose.position.z;

    // VICON_pos_ : relative pos = position in matlab

    // // VICON_uwb_ : relative dis
    VICON_uwb_.leftCols(lopt_)=VICON_uwb_.rightCols(lopt_);
    for (int i=1;i<=uwbnum_;i++){
        VICON_uwb_(i-1,lopt_) = sqrt(pow(VICON_PosUAV_(0,lopt_)-VICON_uwb_ini(0,i-1),2)+pow(VICON_PosUAV_(1,lopt_)-VICON_uwb_ini(1,i-1),2)+pow(VICON_PosUAV_(2,lopt_)-VICON_uwb_ini(2,i-1),2));
    }

    // vicon_relative
    VICON_PosUAV2UWB01_ = VICON_PosUAV_.rightCols(1) - VICON_uwb_ini.col(0);

    // takeoff condition dx or dy or dz >=0.1
    if(LOOP_num_ == count_j_ +1){
        double sum_x_ = VICON_PosUAV_.row(0).sum();
        double sum_y_ = VICON_PosUAV_.row(1).sum();
        double sum_z_ = VICON_PosUAV_.row(2).sum();
        VICON_inital_(0,0) = sum_x_ / VICON_PosUAV_.cols();
        VICON_inital_(1,0) = sum_y_ / VICON_PosUAV_.cols();
        VICON_inital_(2,0) = sum_z_ / VICON_PosUAV_.cols();
    }

    // vicon veluav*********************************************************************
    VICON_VelUAV_.leftCols(lopt_)=VICON_VelUAV_.rightCols(lopt_);
    VICON_VelUAV_(0,lopt_)=VICON_velUAV_cb_.twist.linear.x;
    VICON_VelUAV_(1,lopt_)=VICON_velUAV_cb_.twist.linear.y;
    VICON_VelUAV_(2,lopt_)=VICON_velUAV_cb_.twist.linear.z;

    // VICON_vel_ : relative vel
    VICON_vel_ = VICON_VelUAV_;

    // VICON_ImuUAV_ : relative acc 
    VICON_ImuUAV_.leftCols(lopt_)=VICON_ImuUAV_.rightCols(lopt_);
    if (dt_== 0) {
        // First message received, initialize previousTime
        VICON_ImuUAV_(0,lopt_)=0;
        VICON_ImuUAV_(1,lopt_)=0;
        VICON_ImuUAV_(2,lopt_)=0;
        
        return;
    }
    else{
        VICON_ImuUAV_(0,lopt_)=(VICON_vel_(0,lopt_)-VICON_vel_(0,lopt_-1))/dt_;
        VICON_ImuUAV_(1,lopt_)=(VICON_vel_(1,lopt_)-VICON_vel_(1,lopt_-1))/dt_;
        VICON_ImuUAV_(2,lopt_)=(VICON_vel_(2,lopt_)-VICON_vel_(2,lopt_-1))/dt_;
    }
    // vicon uwb*********************************************************************
    

}



/**
 * @brief solveonce for 4 anchors
 */

void dwe::solveonce()
{
    ROS_INFO("dt: %.4lf seconds", dt_);

    // Initialize matrix in solveonce
    x1_= xt_.rightCols(k_*lopt_+1);
    // MatrixXd x0 = xt_.rightCols(k_*lopt_+2);
    // x1_= x0.leftCols(k_*lopt_+1);
    
    // use vicon
    MatrixXd imu_sol;
    MatrixXd position;
    MatrixXd uwb_sol;
    MatrixXd uwb_est_sol;

    bool Is_cameraB_zeros;
    bool Is_cameraC_zeros;
    bool Is_cameraD_zeros;
    bool Is_cameraE_zeros;
    bool Is_cameraF_zeros;
    bool Is_cameraG_zeros;
    bool Is_cameraH_zeros;

    int uwbnum_solveonce = useduwbnum_;
    if (vicon_flag_ == 1)
    {
        imu_sol = VICON_ImuUAV_.leftCols(k_*lopt_); //imu from vicon
        // imu_sol = MHE_imu_.leftCols(k_*lopt_); //imu from vicon
        position = VICON_PosUWB_ini.leftCols(uwbnum_solveonce);;  // get relative uwb ini from vicon
        MHE_uwb_ = VICON_uwb_.topRows(uwbnum_solveonce);    // test with vicon
        paramUWB_.setConstant(paramR_);
        // paramUWB_.row(0).setConstant(0);
        // paramUWB_.row(3).setConstant(0);
        uwb_sol = VICON_uwb_; // test with sigle achor 

        // paramU_ can set to zero
        uwb_est_sol = MHE_uwb_pos_;
        paramUWB_est_.setConstant(0);
    }
    else{
        // use sensor
        imu_sol = MHE_imu_.leftCols(k_*lopt_); //imu from uav
        // imu_sol = VICON_ImuUAV_.leftCols(k_*lopt_); //imu from vicon
        position = VICON_PosUWB_ini.leftCols(uwbnum_solveonce);  // get relative uwb ini from vicon
        uwb_sol = MHE_uwb_.topRows(uwbnum_solveonce); // test with sigle achor

        uwb_est_sol = MHE_uwb_pos_;
        // paramUWB_est_.setConstant(paramU_);

        // position = pos_uwb_init_;        // get relative uwb ini from param
    }
    // cout<< pos_uwb_init_(0,0) << pos_uwb_init_(0,1)<< pos_uwb_init_(2,3)<<endl;
    

    Eigen::MatrixXd p_sol;
    p_sol.setZero(3*uwbnum_solveonce, k_*lopt_+1); // 存储结果的矩阵
    for(int j = 1; j<=uwbnum_solveonce; j++){
        for (int i = 1; i <= k_*lopt_+1; i++) {
            p_sol.block(3*(j-1),i-1,3,1) = x1_.topRows(3).col(i-1) - position.col(j-1);
        }
    }
    // ROS_WARN("Q22");
    // R = Eigen::DiagonalMatrix<double, 4>(paramUWB_[0],paramUWB_[1],paramUWB_[2],paramUWB_[3]);

    // MatrixXd uwb_sol = MHE_uwb_; .
    // R = paramR_.asDiagonal();

    A << 1, 0, 0, dt_, 0, 0,
                0, 1, 0, 0, dt_, 0,
                0, 0, 1, 0, 0, dt_,
                0, 0, 0, 1-kappa_(0)*dt_, 0, 0,
                0, 0, 0, 0, 1-kappa_(1)*dt_, 0,
                0, 0, 0, 0, 0, 1-kappa_(2)*dt_;

    B << 0.5*pow(dt_,2), 0, 0,
                    0, 0.5*pow(dt_,2), 0,
                    0, 0, 0.5*pow(dt_,2),
                    dt_, 0,  0,  
                    0,  dt_, 0,
                    0,  0, dt_; 

    AA = A;
    for (int i = 1; i < k_; i++) {
        AA = AA*A;
    }  
    // double test_time_start_1 = ros::Time::now().toSec();   //test

    // // we can design a member function to perform a shift operation
    // camera part

    if (abs(cameraA_vector_last(0,lopt_)-camA_(0,lopt_)) < 0.01){
            flag_out_camA_++;
            if(flag_in_camA_==10){
               flag_in_camA_ =0;
            }
        }
    if (flag_out_camA_ >= 50){
        Is_cameraA_zeros_ = true;
        flag_out_camA_ = 0;
        }
    

    if (abs(cameraA_vector_last(0,lopt_)-camA_(0,lopt_)) >= 0.0001){
            flag_in_camA_++;
            if (flag_out_camA_ <=30){
                flag_out_camA_=0;
            }
    }
    if (flag_in_camA_ >= 4){
        Is_cameraA_zeros_ = false;
        flag_in_camA_ = 0;
    }
        cameraA_vector_last = camA_;
    cout<<"flag_in_camA_: "<<flag_in_camA_<<endl;
    cout<<"flag_out_camA_: "<<flag_out_camA_<<endl;
    cout<<"Is_cameraA_zeros_: "<<Is_cameraA_zeros_<<endl;


    for (int i=1;i<=num_state_;i++){ //i=1-20 or 0-19
        // P.block(i, j, rows, cols)          
        // P(i+1 : i+rows, j+1 : j+cols)i,j开始，rows行cols列
        Ex.block((i-1)*6,(i-1)*6,6,6)=Eigen::MatrixXd::Identity(6,6);
        Ex.block((i-1)*6+6*num_state_,(i-1)*6,6,6) = -AA;
        Ex.block((i-1)*6+6*num_state_,(i)*6,6,6) = Eigen::MatrixXd::Identity(6,6);
        // Eigen::MatrixXd p_sols[4] = {p1_sol, p2_sol, p3_sol, p4_sol};

        Eigen::MatrixXd Cs[uwbnum_solveonce];
        Eigen::MatrixXd Ds[uwbnum_solveonce];
        for (int j = 1; j <=uwbnum_solveonce; j++) {
            // ROS_INFO("----------solve1-loop-%d",j);
            Eigen::MatrixXd x_loc = p_sol.block(3*(j-1),i,3,1);
            // p_sols[j-1].col(i);
            double rho = x_loc.norm();
            Cs[j-1] = Eigen::MatrixXd::Zero(4,6);
            Ds[j-1] = Eigen::MatrixXd::Zero(1,6);
            Cs[j-1].block(0, 0, 1, 3) = x_loc.transpose() / rho;
            Ds[j-1].block(0, 0, 1, 3) = x_loc.transpose() / rho;
            Cs[j-1].block(1, 0, 3, 3) = Eigen::MatrixXd::Identity(3, 3);

            // ROS_INFO("----------solve1-loop_before-Ex-%d",j);
            // Ex.block(num_state_*12+1*(i-1),(i)*6,1,6) = Cs[k];
            Ex.block(num_state_*6+6*num_state_+(-3+4*i)+(j-1)*4*num_state_-1,(i)*6,4,6) = Cs[j-1];
        }


        // extra noop_est pos ---- Ex
        Eigen::MatrixXd Cn;
        Cn.setZero(3,6);
        Cn.block(0, 0, 3, 3) = Eigen::MatrixXd::Identity(3,3);
        Ex.block(num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*(i-1),(i)*6,3,6) = Cn;
        // extra noop_est pos ---- Eu ur W
        Eu.block(num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*(i-1),num_state_*6+num_state_*3*k_+uwbnum_solveonce*num_state_+3*(i-1),3,3) = Eigen::MatrixXd::Identity(3,3);
        ur.block(3*k_*num_state_+6*num_state_+uwbnum_solveonce*num_state_+3*(i-1), 0,3,1) = uwb_est_sol.col(i*k_);
        W.block(num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*(i-1),num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*(i-1),3,3) = paramUWB_est_.col(i*k_).asDiagonal();


        Eu.block((i-1)*6,(i-1)*6,6,6) = Eigen::MatrixXd::Identity(6,6);

        for(int j=1;j<=k_;j++){
            Eigen::Matrix<double, 6, 6> Akj;
            Akj.setIdentity(6,6);
            int kj=k_-j;
            for (int q=0;q<kj;q++){
                Akj=Akj*A;
            }
            Eu.block(6*num_state_+(i-1)*6,6*num_state_+(i-1)*3*k_+(j-1)*3,6,3) = Akj*B;
        }
        // Eu.block((num_state_)*12+4*i-4,num_state_*3*k_+6*num_state_+4*i-4,4,4) = Eigen::MatrixXd::Identity(4,4); // multiple achors 


        // multiple achors
        for(int j = 1; j <= uwbnum_solveonce; j++) {
            // camera assistance 
            Eu.block(num_state_*6+num_state_*6+1+(j-1)*num_state_*4+(i-1)*4-1,num_state_*6+num_state_*3+1+(j-1)*num_state_*4+(i-1)*4-1,4,4) = Eigen::MatrixXd::Identity(4,4); // multiple achors
            
            Eigen::VectorXd term1(6); // 创建一个大小为6x1的向量
            term1.setZero(); // 将向量元素都设为0
            // term1.segment(0, 3) = p_sols[j-1].col(i); // 在term1的指定位置赋值
            term1.segment(0, 3) = position.col(j-1); // 在term1的指定位置赋值

            Eigen::VectorXd term2 = uwb_sol(j-1, i * k_) * Eigen::VectorXd::Ones(1) + Ds[j-1] * term1;  

            Eigen::VectorXd  camera_vector(3);

            // camera_vector(0) = VICON_PosUAV_(0,i) - VICON_PosUWB_(0,lopt_);///  
            // camera_vector(1) = VICON_PosUAV_(1,i) - VICON_PosUWB_(1,lopt_);///
            // camera_vector(2) = VICON_PosUAV_(2,i) - VICON_PosUWB_(2,lopt_);///

            // ROS_INFO("VICON_PosUWB_(0,lopt_): %f", VICON_PosUWB_(0,lopt_));
            // ROS_INFO("VICON_PosUWB_(1,lopt_): %f", VICON_PosUWB_(1,lopt_));
            // ROS_INFO("VICON_PosUWB_(2,lopt_): %f", VICON_PosUWB_(2,lopt_));
            double max = 0.5;
            // // ROS_INFO("hello we are here!!!!!!!1");
            if (j == 1){
            camera_vector(0) = camA_(0,i);
            camera_vector(1) = camA_(1,i);
            camera_vector(2) = camA_(2,i);

            // compare camera_vector with last camera_vector

            // 判断这一秒和上一秒的差是否大于0.4,如果是则认为Is_cameraA_zeros为true

            }
            if (j == 2){
            camera_vector(0) = camB_(0,i);
            camera_vector(1) = camB_(1,i);
            camera_vector(2) = camB_(2,i);
            }


            if (j == 3){
            camera_vector(0) = camC_(0,i);
            camera_vector(1) = camC_(1,i);
            camera_vector(2) = camC_(2,i);
            
            }

            if (j == 4 and 1==2){
            camera_vector(0) = camD_(0,i);
            camera_vector(1) = camD_(1,i);
            camera_vector(2) = camD_(2,i);
            

            }

            if (j == 5){
            camera_vector(0) = camE_(0,i);
            camera_vector(1) = camE_(1,i);
            camera_vector(2) = camE_(2,i);
            
            }

            if (j == 6){
            camera_vector(0) = camF_(0,i);
            camera_vector(1) = camF_(1,i);
            camera_vector(2) = camF_(2,i);
                  
            }

            if (j == 7){
            camera_vector(0) = camG_(0,i);
            camera_vector(1) = camG_(1,i);
            camera_vector(2) = camG_(2,i);
            
            }
            
            if (j == 8){
            camera_vector(0) = camH_(0,i);
            camera_vector(1) = camH_(1,i);
            camera_vector(2) = camH_(2,i);
            
            
            }

            Eigen::VectorXd combined_vector = Eigen::VectorXd::Zero(4);
            combined_vector.segment(0, 1)= term2;
            combined_vector.segment(1, 3) = camera_vector;
            ur.block(3*k_*num_state_+6*num_state_+4*num_state_*(j-1)+4*(i-1),0,4,1) = combined_vector;
        }


        // single achor k
        // Eu.block((num_state_)*12+1*(i-1),num_state_*3*k_+6*num_state_+1*(i-1),1,1) = Eigen::MatrixXd::Identity(1,1);  
        // ur(3*k_*num_state_+6*num_state_+1*(i-1),0) = uwb_sol(0,i*k_);
        // ur.block(3*k_*num_state_+6*num_state_+i*4-3,0,3,1) = flow_sol.col(i*k_);

        // W: 320x320 covariance matrix, diag param is set as P,Q,R
        W.block((i-1)*6,(i-1)*6,6,6) = P;
        W.block(6*num_state_+(i-1)*6,6*num_state_+(i-1)*6,6,6) = Q;
        for(int j=1;j<=uwbnum_solveonce;j++){
            // ROS_INFO("----------solve-5_loop-%d",j);
            R = Eigen::MatrixXd::Identity(4,4);

            // ROS_INFO("------------solve-W-start %d",j);
            R(0,0) = paramUWB_(j-1)*1;


            // R(0,0) = 5;
            // if (j == 3){
            //     R(0,0) = 0;
            // }
            // ROS_INFO("paramR_star%f",paramR_star);
            // R(1,1) = param_camera_[0];

            R(1,1) = 1;

            // R(2,2) = param_camera_[1];
            R(2,2) = 1;
            // R(3,3) = param_camera_[2];
            R(3,3) = 5;
            // // // ROS_INFO("------------solve-W-end %d",j);
            double low_bound = 0;

            if (j == 1 && Is_cameraA_zeros_){
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
                // ROS_INFO("Is_cameraA_zeros: %d", Is_cameraA_zeros);
            }

            if (j == 2 && Is_cameraB_zeros){
                // ROS_INFO("Is_cameraB_zeros!!!!!!!!!!!!!!!!!!!!!!!!!!!!!: %d", Is_cameraB_zeros);
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
            }

            if (j == 3 && Is_cameraC_zeros ){
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
            }

            if (j == 4 && Is_cameraD_zeros){
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
            }


            if (j == 5 && Is_cameraE_zeros){
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
            }


            if (j == 6 && Is_cameraF_zeros){
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
            }


            if (j == 7 && Is_cameraG_zeros){
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
            }


            if (j == 8 && Is_cameraH_zeros){
                R(1,1) = low_bound;
                R(2,2) = low_bound;
                R(3,3) = low_bound;
            }

            // if (R(1,1) !=0)
            // {
            //     ROS_INFO("R(1,1): %f", R(1,1));
            // }
            // // ROS_INFO("R: %f", R(1,1));


            W.block(6*num_state_+num_state_*6+(j-1)*num_state_*4+(4*i-3)-1,6*num_state_+num_state_*6+(j-1)*num_state_*4+(4*i-3)-1,4,4) = R;
        }
        
        // W.block(6*num_state_+num_state_*6+i*4-4,6*num_state_+num_state_*6+i*4-4,4,4) = R;

        Eigen::MatrixXd tt(1,order_+1);
        for(int j=1;j<=(order_+1);j++){
            tt(0,j-1) = pow(dt_*i*k_,j-1);
        }
        for(int j=1;j<=6;j++){
            t_matrix.block((i)*6+j-1,(j-1)*(order_+1),1,order_+1) = tt;
        }
    } // we can design a member function to perform a shift operation


    // double test_time_start_2 = ros::Time::now().toSec();   //test
    // ROS_INFO("consumptiom dt1: %.4lf seconds", test_time_start_2 - test_time_start_1);

    // test-
    // for(int j=1;j<=t_matrix.rows();j++){
    //     cout << t_matrix.row(j-1) << endl;
    // } 
    // cout << "end ouput" << endl;

    xtt=x1_.rightCols(x1_.cols()-1);

    for(int j=1;j<=xtt.cols();j=j+1){
        ur.block(0+xtt.rows()*(j-1),0,xtt.rows(),1) = xtt.col(j-1);
    }

    for(int j=1;j<=imu_sol.cols();j=j+1){
        ur.block(xtt.cols()*xtt.rows()+imu_sol.rows()*(j-1),0,imu_sol.rows(),1) = imu_sol.col(j-1);
    }

    Ex_new = Ex * t_matrix;     //320x30
    ar = ((Ex_new.transpose()*W*Ex_new).inverse())*(Ex_new.transpose())*W*Eu*ur;

    // double test_time_start_3 = ros::Time::now().toSec();   //test
    // ROS_INFO("consumptiom dt2: %.4lf seconds", test_time_start_3 - test_time_start_2);

    // Eigen::JacobiSVD<Eigen::MatrixXd> svd2((Ex_new.adjoint()*W*Ex_new).inverse());
    // double cond2 = svd2.singularValues()(0) / svd2.singularValues()(svd2.singularValues().size()-1); 
    // ROS_INFO("cond2: %.4f", cond2);

    xxt_new = t_matrix * ar;

    for(int i=1;i<=num_state_;i++){
        xxe.block(0,i-1,6,1) = xxt_new.block(6*i,0,6,1);
    }

    // revise - update all the estimates
    // tiao bian
//     xt_.conservativeResize(6, LOOP_num_);

//     double diffxt = abs(xt_(0, LOOP_num_-2) - xxt_new(0, lopt_-1));
//     if (diffxt >= 0.5) { // If the difference is big
//         xt_.col(LOOP_num_-1) = (xt_.col(LOOP_num_-2) + xxe.rightCols(1))/2;// Set current value to the previous value
//     }
//     else{
//         xt_.col(LOOP_num_-1)=xxe.rightCols(1);
//     }
//     // xt_.rightCols(12) = xxe;

//     xt_real_.conservativeResize(6, LOOP_num_);
//     xt_real_(0,LOOP_num_-1) = xt_(0,LOOP_num_-1) + VICON_PosUWB01_(0,0);
//     xt_real_(1,LOOP_num_-1) = xt_(1,LOOP_num_-1) + VICON_PosUWB01_(1,0);
//     xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1) + VICON_PosUWB01_(2,0);
//    // xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1);
//     xt_real_(3,LOOP_num_-1) = xt_(3,LOOP_num_-1);
//     xt_real_(4,LOOP_num_-1) = xt_(4,LOOP_num_-1);
//     xt_real_(5,LOOP_num_-1) = xt_(5,LOOP_num_-1);

//     // remain - without vicon
//     xt_.conservativeResize(6, LOOP_num_);
//     // xt_.col(LOOP_num_-1)=xxe.rightCols(1);
//     xt_.rightCols(12) = xxe;

//     xt_real_.conservativeResize(6, LOOP_num_);
//     xt_real_(0,LOOP_num_-1) = xt_(0,LOOP_num_-1) + pos_uwb01_init_(0);
//     xt_real_(1,LOOP_num_-1) = xt_(1,LOOP_num_-1) + pos_uwb01_init_(1);
//     xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1) + pos_uwb01_init_(2);
//    // xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1);
//     xt_real_(3,LOOP_num_-1) = xt_(3,LOOP_num_-1);
//     xt_real_(4,LOOP_num_-1) = xt_(4,LOOP_num_-1);
//     xt_real_(5,LOOP_num_-1) = xt_(5,LOOP_num_-1);

    // double setnum = 0.1;
    // // meanFilter
    // if (abs(xt_(0,LOOP_num_-2)-xt_(0,LOOP_num_-1)) >=setnum || abs(xt_(1,LOOP_num_-2)-xt_(1,LOOP_num_-1))>=setnum || abs(xt_(2,LOOP_num_-2)-xt_(2,LOOP_num_-1))>=setnum) { // If the drone has taken off and diff0 is not zero
    //     xt_.rightCols(lopt_) = meanFilter(xt_.rightCols(lopt_));
    //     xt_real_.rightCols(lopt_) = meanFilter(xt_real_.rightCols(lopt_));
    // }

    // remain
    

    xt_.conservativeResize(6, LOOP_num_);
    xt_.col(LOOP_num_-1)=xxe.rightCols(1);
    // xt_.rightCols(lopt_) = xxe;
    // xt_(0,LOOP_num_-1) = xt_(0,LOOP_num_-1) + bias_pos_(0);
    // xt_(1,LOOP_num_-1) = xt_(1,LOOP_num_-1) + bias_pos_(1);
    // xt_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1) + bias_pos_(2);
    
    xt_real_.conservativeResize(6, LOOP_num_);
    xt_camera.conservativeResize(6,LOOP_num_);

    VICON_uwb_ini(0,0) = -0.99;
    VICON_uwb_ini(1,0) = -0.87;
    VICON_uwb_ini(2,0) =  0.31;
    xt_real_(0,LOOP_num_-1) = xt_(0,LOOP_num_-1) + VICON_uwb_ini(0,0) + bias_pos_(0);
    xt_real_(1,LOOP_num_-1) = xt_(1,LOOP_num_-1) + VICON_uwb_ini(1,0) + bias_pos_(1);
    xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1) + VICON_uwb_ini(2,0) + bias_pos_(2);
   // xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1);
    xt_real_(3,LOOP_num_-1) = xt_(3,LOOP_num_-1);
    xt_real_(4,LOOP_num_-1) = xt_(4,LOOP_num_-1);
    xt_real_(5,LOOP_num_-1) = xt_(5,LOOP_num_-1);


    xt_camera(0,LOOP_num_-1) = camA_(0,lopt_);
    xt_camera(1,LOOP_num_-1) = camA_(1,lopt_);
    xt_camera(2,LOOP_num_-1) = camA_(2,lopt_);

    xt_camera(0,LOOP_num_-1) = xt_camera(0,LOOP_num_-1) + VICON_uwb_ini(0,0) + bias_pos_(0);
    xt_camera(1,LOOP_num_-1) = xt_camera(1,LOOP_num_-1) + VICON_uwb_ini(1,0) + bias_pos_(1);
    xt_camera(2,LOOP_num_-1) = xt_camera(2,LOOP_num_-1) + VICON_uwb_ini(2,0) + bias_pos_(2);
//     xt_real_(0,LOOP_num_-1) = xt_(0,LOOP_num_-1) + pos_uwb01_init_(0,0);
//     xt_real_(1,LOOP_num_-1) = xt_(1,LOOP_num_-1) + pos_uwb01_init_(1,0);
//     xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1) + pos_uwb01_init_(2,0);
//    // xt_real_(2,LOOP_num_-1) = xt_(2,LOOP_num_-1);
//     xt_real_(3,LOOP_num_-1) = xt_(3,LOOP_num_-1);
//     xt_real_(4,LOOP_num_-1) = xt_(4,LOOP_num_-1);
//     xt_real_(5,LOOP_num_-1) = xt_(5,LOOP_num_-1);

    // takeoff
    // if(!get_init_imu){
    //     get_init_imu = true;
    //     xt_inital_(0,0) = xt_(0, LOOP_num_-1);
    //     xt_inital_(1,0) = xt_(1, LOOP_num_-1);
    //     xt_inital_(2,0) = xt_(2, LOOP_num_-1);
    // }

    // if(get_init_imu && !check_for_init_){
    //     if(abs(xt_(0,LOOP_num_-1) - xt_inital_(0, 0)) >= 0.05 || abs(xt_(1,LOOP_num_-1) - xt_inital_(1, 0)) >= 0.05 || abs(xt_(2,LOOP_num_-1) - xt_inital_(2,0)) >= 0.05){
    //         check_for_init_ = true;
    //         ROS_WARN("start!!!!!!!!!!!!!");
    //     }

    //     Eigen::MatrixXd xt_pre = xt_.rightCols(lopt_+1);
    //     xt_.rightCols(lopt_) = xt_pre.leftCols(lopt_);
    //     Eigen::MatrixXd xt_real_pre = xt_real_.rightCols(lopt_+1);
    //     xt_real_.rightCols(lopt_) = xt_real_.leftCols(lopt_);
    // }
 

    // meanFilter
    // int mean_num = 3;
    // Eigen::MatrixXd xt_real_meanFilter = meanFilter(xt_real_.topRows(3).rightCols(mean_num));
    // xt_real_.topRows(3).rightCols(1)= xt_real_meanFilter.rightCols(1);
    // Eigen::MatrixXd xt_meanFilter = meanFilter(xt_.topRows(3).rightCols(mean_num));
    // xt_.topRows(3).rightCols(1)= xt_meanFilter.rightCols(1);

    // Eigen::MatrixXd xt_real_meanFilter = meanFilterWithSlidingWindow(xt_real_.topRows(3).rightCols(lopt_),4);
    // xt_real_.topRows(3).rightCols(1)= xt_real_meanFilter.rightCols(1);
    // Eigen::MatrixXd xt_meanFilter = meanFilterWithSlidingWindow(xt_.topRows(3).rightCols(lopt_),4);
    // xt_.topRows(3).rightCols(1)= xt_meanFilter.rightCols(1);

    return;

}